import launch
import launch.event_handlers
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
import launch_ros.parameter_descriptions

def generate_launch_description():
    robot_name_in_model='newrobot'
    urdf_default_path=get_package_share_directory('myrobot_description')
    default_model_path = urdf_default_path+'/urdf/newrobot/newrobot.urdf.xacro'
    default_world_path = urdf_default_path+'/world/custom_room.world'
    action_declare_arg_mode_path=launch.actions.DeclareLaunchArgument(
        name='model',default_value=str(default_model_path),description='URDF的绝对路径'
    )

    robot_description=launch_ros.parameter_descriptions.ParameterValue(
        value=launch.substitutions.Command(
            ['xacro ',launch.substitutions.LaunchConfiguration('model')]
        ),
        value_type=str,
    )

    robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description}]
    )

    launch_gazebo = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource([get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']),
        launch_arguments=[('world',default_world_path),('verbose','true')]
    )

    spawn_entity_node=launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic','/robot_description','-entity',robot_name_in_model,]
    )

    load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2','control','load_controller','--set-state','active','newrobot_joint_state_broadcaster'],
        output='screen'
    )

    load_newrobot_effort_controller = launch.actions.ExecuteProcess(
        cmd=['ros2','control','load_controller','--set-state','active','newrobot_effort_controller'],
        output='screen'
    )

    load_newrobot_diff_drive_controller = launch.actions.ExecuteProcess(
        cmd=['ros2','control','load_controller','--set-state','active','newrobot_diff_drive_controller'],
        output='screen'
    )

    launch_description = launch.LaunchDescription([
        action_declare_arg_mode_path,
        robot_state_publisher,
        launch_gazebo,
        spawn_entity_node,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=spawn_entity_node,
                on_exit=[load_joint_state_controller],
            )
        ),   
        # launch.actions.RegisterEventHandler(
        #     event_handler=launch.event_handlers.OnProcessExit(
        #         target_action=load_joint_state_controller,
        #         on_exit=[load_newrobot_effort_controller],
        #     )
        # ), 
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=load_joint_state_controller,
                on_exit=[load_newrobot_diff_drive_controller],
            )
        ),                                                        
    ])

    return launch_description